//
// Created by mordimer on 15.05.17.
//

#ifndef PROJEKTMAGISTERSKI_POINTCLOUD_H
#define PROJEKTMAGISTERSKI_POINTCLOUD_H

#include <Point3D.h>
#include <PosOrient.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <string>
#include <fstream>

using PointType = pcl::PointXYZINormal;

class Point3DCloud {
private:
    std::vector<Point3D> points3D;
    float intensity;
    int label;

    void transformPoint3D(Point3D &point3D, const PosOrient &posOrient) 
    {
        auto rotMatrix = posOrient.getQuaternion().toRotationMatrix();
        auto translation = posOrient.getPosition();
        point3D.position = (rotMatrix * point3D.position) + translation;
    }

public:
    Point3DCloud(){}
    Point3DCloud(std::string filepath) { loadXYZIFromFile(filepath); }
    Point3DCloud(std::vector<Point3D> points) { points3D = points; }

    int size() { return this->points3D.size(); }
    void push_back(Point3D point3D){ points3D.push_back(point3D);}
    void clear(){ points3D.clear();}
    const std::vector<Point3D> &getPoints3D() const {return points3D;}

    void setPoints(std::vector<Point3D> _points) { points3D = _points;}
    void setLabel(int value) { label = value; }
    inline void toPointCloud(pcl::PointCloud<PointType>::Ptr cloud){
        if(!cloud){
            std::cout << "cloud ptr is null!" << std::endl;
            return;
        }
        cloud->clear();
        for(auto point : this->points3D){
            PointType p;
            p.x = point.position(0);
            p.y = point.position(1);
            p.z = point.position(2);
            p.intensity = point.intensity;
            p.curvature = point.curvature;
            cloud->push_back(p);
        }
    }

    inline void fromPointCloud(pcl::PointCloud<PointType>::Ptr &cloud)
    {
        if(!cloud){
            std::cout << "cloud ptr is null!" << std::endl;
            return;
        }
        for(auto p : cloud->points)
            this->points3D.push_back(Point3D(p.x, p.y, p.z, p.intensity, p.curvature));
        
    }

    void transform(const PosOrient &posOrient)
    {
        for(auto &point : this->points3D)
            transformPoint3D(point, posOrient);
    }

    void translate(Vector3d &translation){
        for(auto &p : this->points3D)
            p.position += translation;
    }

    void loadXYZIFromFile(string path)
    {
        ifstream input(path);
        
        for(string line; getline(input, line);){
            istringstream in(line);
            Point3D p;
            for(int i=0; i<3; ++i)
                in >> p.position(i);
            in >> p.red;
            p.blue = p.red;
            p.green = p.red;
            this->points3D.push_back(p);
        } 
    }

    void loadXYZITFromFile(string path)
    {
        ifstream input(path);
        
        for(string line; getline(input, line);){
            istringstream in(line);
            Point3D p;
            for(int i=0; i<3; ++i)
                in >> p.position(i);
            in >> p.intensity >> p.red;
            p.blue = p.red;
            p.green = p.red;
            this->points3D.push_back(p);
        }
    }
};


#endif //PROJEKTMAGISTERSKI_POINTCLOUD_H
